#include   "RoboDevice.h"
#include "vex.h"
using namespace vex;
using signature = vision::signature;
using code = vision::code;

namespace RoboDevice{
    
    // Brain
    vex::brain Brain;
    
    // Controller
    vex::controller Controller(primary);
    
    // Motor
    const int LeftFrontMotorPort  = vex::PORT19;
    const int LeftBackMotorPort   = vex::PORT20;
    const int RightFrontMotorPort = vex::PORT7;
    const int RightBackMotorPort  = vex::PORT9;
    const int RoboRollerPort      = vex::PORT8;
    // const int RoboArmPort         = vex::PORT5;
    const int RoboClampHeadUpPort   = vex::PORT1;
    const int RoboClampHeadDownPort   = vex::PORT11;
    const int RoboClampBackPort   = vex::PORT18;//未测
    
    const vex::gearSetting ChassisMotorGearsettingGreen = vex::gearSetting::ratio18_1;
    const vex::gearSetting ChassisMotorGearsettingRed = vex::gearSetting::ratio36_1;

    vex::motor LeftFrontMotor (LeftFrontMotorPort  ,ChassisMotorGearsettingGreen, true);
    vex::motor LeftBackMotor  (LeftBackMotorPort   ,ChassisMotorGearsettingGreen, true);
    vex::motor RightFrontMotor(RightFrontMotorPort ,ChassisMotorGearsettingGreen, false);
    vex::motor RightBackMotor (RightBackMotorPort  ,ChassisMotorGearsettingGreen, false);

    vex::motor RoboRoller(RoboRollerPort ,ChassisMotorGearsettingGreen,true);
    //vex::motor RoboArm (RoboArmPort  ,ChassisMotorGearsetting,true);//旋转方向待定
    // Sensor
    vex::motor RoboClampHeadUp (RoboClampHeadUpPort  ,ChassisMotorGearsettingRed,false);
    vex::motor RoboClampHeadDown (RoboClampHeadDownPort  ,ChassisMotorGearsettingRed,true);
    vex::motor RoboClampBack (RoboClampBackPort  ,ChassisMotorGearsettingRed,false);//旋转方向待定

    const int InertialPort = PORT10;
    vex::inertial Inertial(InertialPort);

    /*vex-vision-config:begin*/
    signature VisionSensor__YELLOW_CUBE = signature (1, 1495, 2071, 1783, -4317, -3951, -4134, 7.1, 0);
    signature VisionSensor__BLUE_CUBE = signature (2, -3473, -2809, -3141, 10893, 16507, 13700, 2.3, 0);
    signature VisionSensor__RED_CUBE = signature (3, 7589, 10241, 8915, -1213, -595, -904, 2.5, 0);
    vision VisionSensor = vision (PORT16, 50, VisionSensor__YELLOW_CUBE, VisionSensor__BLUE_CUBE, VisionSensor__RED_CUBE);
/*vex-vision-config:end*/

    //Timer
    vex::timer SystemTimer;

    //Distance Measure
    vex::distance RoboDistance = distance(PORT3);
    
    // Function
    void RoboDeviceInit(){
        Inertial.calibrate(2);
        while(Inertial.isCalibrating()){
          wait(10, msec);
        }
        wait(20,msec);
    }
}